#pragma region Declarations
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
#include <math.h>
#include <vector>
#include <stack>
#include <list>
#include <queue>
#include <assert.h>
#include "byuGL4.h"
#include "vecmat.h"
#include "Camera.h"
#include "Input.h"
#include "Scene.h"
#include "Scene1.h"
using namespace std;

#pragma endregion

#pragma region Globals
Camera camera;
Scene *scene = NULL;
Input input;
float t=0;
static long lastTime = 0;
#pragma endregion

#pragma region Function Files
#include "ImageFunctions.h"
#include "InputFunctions.h"
#include "DrawFunctions.h"
#pragma endregion

void idle() {
	t += .1;
	glutPostRedisplay();
}

bool CanMove(vec<float, 3> pos, vec<float, 3> front, vec<float, 3> right)
{
	bool result = true;
	list<Triangle*> collisionShapes;
	scene->GetSphere(&collisionShapes, 1);
	vec<float, 3> newpos; newpos = pos + front + right;
	for(list<Triangle*>::iterator iter = collisionShapes.begin(); iter != collisionShapes.end(); ++iter)
	{
		ColoredPoint p = (*iter)->GetPoint1();
		vec<float, 3> point; point = p.position[0], p.position[1], p.position[2];
		vec<float, 3> norm; norm = (*iter)->GetNormal();
		float oldv = norm.inner(pos) - norm.inner(point);
		float newv = norm.inner(newpos) - norm.inner(point);
		if((oldv < 0 && newv > 0) || (oldv > 0 && newv < 0))
			result = false;
	}
	return result;
}
float GetRayDistanceToPlane(vec<float,3> rayStart, Triangle &T)
{
    // dist = n.(p-a) / |n|
    // see http://omega.albany.edu:8008/calc3/distance-to-point-dir/ptoplane-maple.html
    //
    // also note that it is assumed that the normal is already
    // normalized (excuse the pun)
    return T.GetNormal().inner(rayStart - T.GetPoint1().position.ToVec3());
}
bool PointIsOnTriangle(vec<float,3> rayStart, float distanceToPlane, Triangle &T)
{
    // please see http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#Segment-Triangle
    // for more details on the algorithm
    vec<float,3> rayEnd = rayStart + T.GetNormal() * -1 * distanceToPlane;
    vec<float,3> u = T.GetPoint2().position.ToVec3() - 
                     T.GetPoint1().position.ToVec3();
    vec<float,3> v = T.GetPoint3().position.ToVec3() - 
                     T.GetPoint1().position.ToVec3();
    vec<float,3> w = rayEnd - T.GetPoint1().position.ToVec3();

    //float s1 = w.inner(T.GetNormal().cross(v)) / u.inner(T.GetNormal().cross(v));
    //float f1 = w.inner(T.GetNormal().cross(u)) / v.inner(T.GetNormal().cross(u));

    float uu, uv, vv, uw, vw;
    uu = u.inner(u);
    uv = u.inner(v);
    vv = v.inner(v);
    uw = u.inner(w);
    vw = v.inner(w);
    float s2 = (uv*vw - vv*uw) / (uv*uv - uu*vv);
    float f2 = (uv*uw - uu*vw) / (uv*uv - uu*vv);

    return s2>=0 && f2>=0 && s2+f2<=1;
}
bool CollisionWasDetected()
{
    float radius = 2;
	list<Triangle*> collisionShapes;
	scene->GetSphere(&collisionShapes, radius);
    vec<float, 3> newPos = camera.getTranslatedLocation(
        input.getStepRight(),0,input.getStepFoward());
	for(list<Triangle*>::iterator iter = collisionShapes.begin(); 
        iter != collisionShapes.end(); ++iter)
	{
        float distanceToPlane = GetRayDistanceToPlane(newPos, **iter);
        if (abs(distanceToPlane) <= radius  &&
            PointIsOnTriangle(newPos, distanceToPlane, **iter))
            return true;
	}
	return false;
}

void timed(int t) 
{
	glutTimerFunc(t,timed,t);

	long dt = glutGet(GLUT_ELAPSED_TIME) - lastTime;
	lastTime += dt;
	double seconds = dt / 1000.0;
	
	// handle Translation
	// if(CanMove(camera.loc, camera.face * input.getStepFoward(), (camera.face.cross(camera.up)).normalize() * input.getStepRight()))
    if (CollisionWasDetected() == false)
	{
		camera.translateCameraSpace(input.getStepRight(),0,0);
		camera.translateCameraSpace(0,0,input.getStepFoward());
	}

	// handle Camera Rotation
	camera.rotateCameraSpace(INPUT_ROTATION_ANGLE_UP, input.getUpRotation(), 0, 0);
	camera.rotateCameraSpace(INPUT_ROTATION_ANGLE_RIGHT, 0, input.getRightRotation(), 0);
	
	glutPostRedisplay();
}

int main(int argc, char *argv[]) 
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
    W=800; H=800;
	glutInitWindowSize(W,H);

	// glutFullScreen();

	udraw    = draw;
	ureshape = reshape;
	
	glutInitWindowPosition(0,0);
	owin = glutCreateWindow("Sam and Dan's OpenGL Game");

	// Initialize Display Functions
	glutReshapeFunc(resize);
	glutDisplayFunc(udraw);
	
	// Initialize Idle Function
	glutIdleFunc(idle);

	// Initialize Keyboard Listeners
	glutIgnoreKeyRepeat(1);
	glutKeyboardFunc(key);
	glutKeyboardUpFunc(keyUp);
	glutSpecialFunc(specialKey);
	glutSpecialUpFunc(specialKeyUp);

	// Initialize Mouse Listeners
    //HCURSOR cursor = LoadCursor(NULL, "blank.cur");
    //SetCursor(cursor);
    input.centerH = H/2 - 30;
    input.centerW = W/2 - 4;
    input.centerMouseCoords();
	glutPassiveMotionFunc(mouseMoved);
	glutMotionFunc(mouseDragged);
	glutMouseFunc(mouseButton);

    // sky box textures
    BuildTexture("pos_y.bmp",    GetTexture(3));
    BuildTexture("neg_y.bmp",    GetTexture(4));
    BuildTexture("neg_x.bmp",    GetTexture(5));
    BuildTexture("pos_x.bmp",    GetTexture(6));
    BuildTexture("neg_z.bmp",    GetTexture(7));
    BuildTexture("pos_z.bmp",    GetTexture(8));

    scene = new Scene1();
    scene->Build();
	scene->ProcessOctree(90, ((float)W)/((float)H));

	glutTimerFunc(0,timed,50);

	glutMainLoop();

	return 0;
}
